#!/usr/bin/python -O
"""
 ----------------------------------------------------------------------------
 MODULE: taw_3DMath.py		- perform 3-Space math functions.
 ----------------------------------------------------------------------------
 NOTE: taw refers to Todd A. Warner, the original author.

 These functions perform various 3D or vector math:
	flt2Int( f )		  - Convert a float to an integer (remedial
				    rounding).
	flt2IntVec( v )	  - Converts a vector defined by floats to a
				    vector defined by ints (remedial rnding).
	dist2Pts( p1, p2 )	  - Returns the distance between two points.
	minRDist( v1, v2 )        - Retrns smallest rectangular distance between
				    two vectors (or points).
	limVec( v, lim )	  - Limits the max size of any component in a
				    vector. I.e., if the largest component is 
				    largr than lim, the vector is scaled down to
				    that lim (limit).
	addVec( v1, v2 )	  - Returns v1+v2.
	subVec( v1, v2 )	  - Returns v1-v2.
	minRDist( v1, v2 )	  - Retrns smallest rectangular distance between
				    two vectors (or points).
	dotProd( v1, v2 )	  - Returns the dot product of two vectors.
	calcMagnitude( v1 )	  - Calculate the magnitude of a vector.
	calcApproxAngle( v1, v2 ) - Calculate the approximate angle between two
				  vectors (NOTE: don't use yet.)
	cranslate( p, tp )	- Returns a translate point (more or less the
				  same as AddVec(v1, v2).
	scale( p, factor )	- Returns the scaled point (or vector).
 ----------------------------------------------------------------------------
 License: GPL: http://www.gnu.org/copyleft/gpl.html

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Library General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 ----------------------------------------------------------------------------
 Created: 20000605	-taw: Todd Warner <taw@pobox.com>
 Updated: 20010321	-taw
 ----------------------------------------------------------------------------
"""
from math import sqrt, pow, fabs, pi, ceil
from copy import deepcopy



def flt2Int( f ):
    """
    This function converts a float to an integer.
    It is done only at the remedial level. It looks at the tenth digit and
    rounds according to the, if 5+ then round up, else, round down.

    PRE:  A float.
    POST: none.
    RET:  A integer vector.
    
    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    x = int( f * 10 )   -   int( f )   *   10

    if x >= 5 or x <= -5:
        if x < 0:
            x = -1
        else:
            x = 1
    f = int( f + x )
    return f



def flt2IntVec( v ):
    """
    This function converts a vector of floats to one of integers.
    It is done only at the remedial level. It looks at the tenth digit and
    rounds according to the, if 5+ then round up, else, round down.
    
    PRE:  A float vector.
    POST: none.
    RET:  A integer vector.
    
    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    for i in xrange( 3 ):
	# Could have used FltToInt here... did this for one less function call.
        x = int( v[i] * 10 )   -   int( v[i] )   *   10
        if x >= 5 or x <= -5:
            if x < 0:
                x = -1
            else:
                x = 1
        v[i] = int( v[i] + x )
    return v


def dist2Pts( p1, p2 ):
    """
    This function will compute the distance between two points.

    PRE:  p1 & p2 are points implemented as lists with three members (x, y, z).
    POST: none.
    RET:  The distance as a FLOAT!
          -1.0 if error.
    
    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    if not p1 or not p2:
	return None

    #print type(p1)
    dx = p1[0] - p2[0]
    dy = p1[1] - p2[1]
    dz = p1[2] - p2[2]
    return sqrt( dx*dx  +  dy*dy  +  dz*dz )
    # dist2PtsI( p1, p2 )
    
    
def maxRDist( v1, v2 ):
    """
    The will compute the largest *RECTANGULAR* distance between two vectors.
    
    PRE:  v1 & v2 are 3D vectors implemented as lists with three members
          (x, y, z).
    POST: none.
    RET:  The max distance as a FLOAT!
          -1.0 if error.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    dx = fabs( v1[0] - v2[0] ) 
    dy = fabs( v1[1] - v2[1] ) 
    dz = fabs( v1[2] - v2[2] ) 

    if dx < dy:
        m = dy
    else:
        m = dx

    if m < dz:
        m = dz

    return m
    # maxRDist( v1, v2 )
    
    

def limVec( v, lim ):
    """
    This will take the largest component, see if smaller than lim. If so,
    then scale everything down to the factor of that multiple. I.e. making
    a vector with largest limit <= lim.

    PRE:  A vector: v.
    	  A limiting value: lim.
    POST: none.
    RET:  A vector with largest component vector <= lim.
    
    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    vl = deepcopy( v )
    try:
        m = fabs( v[0] )
        if m < fabs( v[1] ):
            m = fabs( v[1] )
        if m < fabs( v[2] ):
            m = fabs( v[2] )
    except OverflowError, e:
        print e
        print "m:", m
        print "v:", v
        print "lim:", lim
        
    if m <= lim:
        return vl
        
    f = lim/m
    vl = scale( v, f )
    return vl



def addVec( v1, v2 ):
    """
    Add two vectors together.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    #DEBUG: print v1, v2
    v = [  v1[0] + v2[0],
	   v1[1] + v2[1],
	   v1[2] + v2[2]  ]
    return v
    
    
def subVec( v1, v2 ):
    """
    Subtract two vectors: v1 - v2.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    v = [  v1[0] - v2[0],
	   v1[1] - v2[1],
	   v1[2] - v2[2]  ]
    return v

def dotProd( v1,v2 ):
    """
    Calculate the dot product of two vectors.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    return v1[0] * v2[0]   +   v1[1] * v2[1]   +   v1[2] * v2[2]
    
    
def calcMagnitude( v1 ):
    """
    Calculate the magnitude of a vector.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    return sqrt( v1[0] * v1[0]   +   v1[1] * v1[1]   +   v1[2] * v1[2] )


def calcApproxAngle( v1, v2 ):
    """
    DON'T USE THIS.... it is not finished.
    Calculate the approximate angle between two vectors.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2001-03-21	-taw
    """
    # Didn't use calcMagnitude in order to boost speed.
    v1Mag = v1[0] * v1[0]   +   v1[1] * v1[1]   +   v1[2] * v1[2]
    v2Mag = v2[0] * v2[0]   +   v2[1] * v2[1]   +   v2[2] * v2[2]

    v = dotProd( v1, v2 )
    cosTheta = v / ( v1Mag * v2Mag )
    
    angTable = [
		0,		##    0,
		0.523598775,	##    pi/6,
		0.785398163,	##    pi/4,
		1.047197551,	##    pi/3,
		1.570796327,	##    pi/2,
		2.094395102,	##    2*pi/3,
		2.35619449,	##    3*pi/4,
		2.617993878,	##    5*pi/6,
		3.141592654,	##    pi,
		4.71238898,	##    3*pi/2,
		6.283185307	##    2*pi
    	]
    	
    index = -1
    smallest = cosTheta

    for i in angTable:
        index = index +1
        diff = fabs( cosTheta - i )
        if diff < smallest:
            smallest = diff

    angTable = [
		0,	##    0,
		30,	##    pi/6,
		45,	##    pi/4,
		60,	##    pi/3,
		90,	##    pi/2,
		120,	##    2*pi/3,
		135,	##    3*pi/4,
		150,	##    5*pi/6,
		180,	##    pi,
		270,	##    3*pi/2,
		360	##    2*pi
    	]
    smallest = 0
    return smallest
    
    
    
def translate( p, tp ):
    """
    This function will compute a translated point.
    NOTE: Same as AddVec(v1, v2)
    PRE:  p = pt to translate.
          t = pt to translate with.
    POST: none.
    RET:  A new, translated point.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    newP = [  p[0] + tp[0],
              p[1] + tp[1],
              p[2] + tp[2]  ]
    return newP
    # translate( p, tp ):
   


def scale( p, factor ):
    """
    This function will compute a scale.
    PRE:  p      = pt to translate.
          factor = factor to scale by.
    POST: none.
    RET:  A new, scaleed point.

    Created:	2000-06-07	-taw: Todd Warner <taw@pobox.com>
    Updated:	2000-06-07	-taw
    """
    newP = [ p[0] * factor,
             p[1] * factor,
             p[2] * factor ]
    return newP
    # scale( p, tp ):
   


# ----------------------------------------------------------------------------
if __name__ == '__main__':
# ----------------------------------------------------------------------------
    print "TESTING OUT THE taw_3DMath.py module"
    p1 = [4,2,5]
    p2 = [3,5,2]
    print dist2Pts( p1,p2 ), p1, p2
    p1 = translate( p1, p2 )
    print p1, p2
else:
    pass	# imported as a module
# ----------------------------------------------------------------------------



# ----------------------------------------------------------------------------
# ----------------------------------------------------------------------------
